﻿using Checkpoint2XNA.Utilities;
using Microsoft.Xna.Framework;
using System;
using System.Diagnostics;

namespace Checkpoint2XNA.Models
{
    public class SolidSphere : Model3D
    {
        public Vector3 Position { get; set; }
        public float Radius { get; set; }
        public float RadiusSquared { get; set; }

        public Color color;

        public SolidSphere(Vector3 p, float r, Color c)
        {
            Position = p;
            Radius = r;
            RadiusSquared = r * r;
            color = c;
        }

        public override void MoveTo(Vector3 point)
        {
            Position = point;
        }


        public override Color GetColorAt(Vector3 point)
        {
            return color;
        }


        public override Vector3 GetNormal(Vector3 point)
        {
            //var invRad = 1.0 / Radius;

            var normal = new Vector3((point.X - Position.X),
                                       (point.Y - Position.Y),
                                       (point.Z - Position.Z));
            normal.Normalize();
            return normal;
        }


        /// <summary>
        /// 
        /// A = Xd^2 + Yd^2 + Zd^2 // is 1 when using normalized vector
        /// B = 2 * (Xd * (X0 - Xc) + Yd * (Y0 - Yc) + Zd * (Z0 - Zc))
        /// C = (X0 - Xc)^2 + (Y0 - Yc)^2 + (Z0 - Zc)^2 - Sr^2
        /// </summary>
        /// <param name="origin"></param>
        /// <param name="normalVector"></param>
        /// <returns></returns>
        public override double RayCollide(Vector3 origin, Vector3 normalVector)
        { 
            float b = Vector3.Dot(normalVector, origin - Position);
            float c = Vector3.DistanceSquared(origin, Position) - RadiusSquared;
            float d = b * b - c;

            if (d < 0.0f)
            {
                return -1.0f;
            }
            var t0 = (-b - Math.Sqrt(d));
            var t1 = (-b + Math.Sqrt(d));
            return t0 > 0 ? t0 : t1;

        }

    }
}
